#include "../Device/speed.h"

#include <math.h>

#include "main.h"

#define MECHANICAL_A_DIRECTION (-1)
#define MECHANICAL_B_DIRECTION (1)

extern float motor_a_counter, motor_b_counter, motor_a_speed, motor_b_speed;
float motor_a_speed_d, motor_b_speed_d, yaw_setpoint;

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
  switch (GPIO_Pin) {
    case GPIO_PIN_0:
      if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1))
        motor_a_counter += MECHANICAL_A_DIRECTION;
      else
        motor_a_counter -= MECHANICAL_A_DIRECTION;
      break;
    case GPIO_PIN_1:
      if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0))
        motor_a_counter -= MECHANICAL_A_DIRECTION;
      else
        motor_a_counter += MECHANICAL_A_DIRECTION;
      break;
    case GPIO_PIN_4:
      if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5))
        motor_b_counter += MECHANICAL_B_DIRECTION;
      else
        motor_b_counter -= MECHANICAL_B_DIRECTION;
      break;
    case GPIO_PIN_5:
      if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4))
        motor_b_counter -= MECHANICAL_B_DIRECTION;
      else
        motor_b_counter += MECHANICAL_B_DIRECTION;
      break;
  }
}

void Speed_Init() {
  motor_a_counter = 0;
  motor_b_counter = 0;
}

void Speed_Scan() {
  float motor_a_speed_old = motor_a_speed;
  float motor_b_speed_old = motor_b_speed;
  motor_a_speed = motor_a_counter /
                  (SPEED_MAX_PULSE_NUMBER * SPEED_SENSOR_NUMBER) *
                  SPEED_SCAN_FREQ;
  motor_b_speed = motor_b_counter /
                  (SPEED_MAX_PULSE_NUMBER * SPEED_SENSOR_NUMBER) *
                  SPEED_SCAN_FREQ;
  motor_a_speed_d = (motor_a_speed - motor_a_speed_old) * SPEED_SCAN_FREQ;
  motor_b_speed_d = (motor_b_speed - motor_b_speed_old) * SPEED_SCAN_FREQ;
  motor_a_counter = 0;
  motor_b_counter = 0;
}
